
void YawController::timerCallback()
{
  try {
    double current_yaw = getCurrentYaw();
    
    // Calculate error considering angle wrapping
    double yaw_error = normalizeAngle(target_yaw_ - current_yaw);
    
    // Check if we're within tolerance
    if (std::abs(yaw_error) <= yaw_tolerance_) {
      publishVelocity(0.0);  // Stop rotation
      return;
    }
    
    // Calculate time step
    rclcpp::Time current_time = this->get_clock()->now();
    double dt = (current_time - last_time_).seconds();
    last_time_ = current_time;
    
    // Compute PID output
    double angular_velocity = pid_controller_->compute(target_yaw_, current_yaw, dt);
    
    // Apply minimum velocity threshold
    if (std::abs(angular_velocity) > 0.0 && std::abs(angular_velocity) < min_angular_velocity_) {
      angular_velocity = (angular_velocity > 0) ? min_angular_velocity_ : -min_angular_velocity_;
    }
    
    publishVelocity(angular_velocity);
    
    // Log status
    RCLCPP_DEBUG(this->get_logger(), 
      "Current yaw: %.2f°, Target yaw: %.2f°, Error: %.2f°, Output: %.3f rad/s",
      current_yaw * 180.0 / M_PI,
      target_yaw_ * 180.0 / M_PI,
      yaw_error * 180.0 / M_PI,
      angular_velocity);
      
  } catch (const tf2::TransformException &ex) {
    RCLCPP_WARN(this->get_logger(), "Could not get transform: %s", ex.what());
    publishVelocity(0.0);  // Stop on error
  }
}

void YawController::targetYawCallback(const std_msgs::msg::Float64::SharedPtr msg)
{
  target_yaw_ = normalizeAngle(msg->data);
  pid_controller_->reset();  // Reset PID when target changes
  
  RCLCPP_INFO(this->get_logger(), 
    "New target yaw set: %.2f degrees", target_yaw_ * 180.0 / M_PI);
}

double YawController::getCurrentYaw()
{
  geometry_msgs::msg::TransformStamped transform_stamped;
  
  // Get the transform from map to base_footprint
  transform_stamped = tf_buffer_->lookupTransform(
    map_frame_, base_frame_, tf2::TimePointZero);
  
  // Extract yaw from quaternion
  tf2::Quaternion q(
    transform_stamped.transform.rotation.x,
    transform_stamped.transform.rotation.y,
    transform_stamped.transform.rotation.z,
    transform_stamped.transform.rotation.w);
  
  double roll, pitch, yaw;
  tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
  
  return yaw;
}

double YawController::normalizeAngle(double angle)
{
  while (angle > M_PI) {
    angle -= 2.0 * M_PI;
  }
  while (angle < -M_PI) {
    angle += 2.0 * M_PI;
  }
  return angle;
}

void YawController::publishVelocity(double angular_velocity)
{
  geometry_msgs::msg::Twist twist_msg;
  twist_msg.linear.x = 0.0;
  twist_msg.linear.y = 0.0;
  twist_msg.linear.z = 0.0;
  twist_msg.angular.x = 0.0;
  twist_msg.angular.y = 0.0;
  twist_msg.angular.z = angular_velocity;
  
  velocity_publisher_->publish(twist_msg);
}

}  // namespace yaw_angle_controller

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<yaw_angle_controller::YawController>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}